# To add a new cell, type '# %%'
# To add a new markdown cell, type '# %% [markdown]'
# %%
from serial.tools import list_ports
from test_final_maybe import camera_active
from pydobot.dobot import Dobot
import time

port = list_ports.comports()[0].device
device = Dobot(port=port, verbose=False)
device._set_queued_cmd_clear()
device.clear_all_alarms_state()


# %%
device.rotate_to(-90.0,0.0,-10.0,0.0)


# %%
# device.go_home()

positions = camera_active()
print(positions)


# %%
for position in positions:
    device.rotate_to(-90.0,0.0,-10.0,0.0)
    (x, y, z, r, j1, j2, j3, j4) = device.pose()
    device.move_to(x , y,0, r, wait=True)
    base_x = x
    x,grab_y = position
    grab_position_adjust = 27
    grab_x = x + grab_position_adjust + base_x# limite <100
    grab_z = -31
    device.move_to(grab_x,grab_y,z,r,wait=True)
    device.move_to(grab_x,grab_y,grab_z,r,wait=True)
    device.suck(True)
    device.rotate_to(0.0,0.0,0.0,0.0)
    device.suck(False)
    device.rotate_to(-90.0,0.0,-10.0,0.0)
    # device.move_to(300 , -83, grab_z, r, wait=True)

# grab_y = 0 + grab_position_adjust
# grab_z = -31
# device._set_queued_cmd_clear()
# (x, y, z, r, j1, j2, j3, j4) = device.pose()
# print(f'x:{x} y:{y} z:{z} j1:{j1} j2:{j2} j3:{j3} j4:{j4}')
# device.go_grab_home()  # 200, 0
# device.move_to(300 , -83, grab_z, r, wait=True) #first box relative -101,247
# device.move_to(grab_x, grab_y, grab_z, r, wait=True)
# device.suck(True)
# time.sleep(1)
# device.go_grab_home()
device._set_queued_cmd_clear()


# %%



